/***************************************************************************
				Assembly.cpp
                             -------------------
Copyright (c) 2009-2010 
Author: Kishor D. Bhalerao (kishor8dm@gmail.com)
Contributors: 

This file is associated with the Ph.D. dissertation,"ON METHODS FOR EFFICIENT
AND ACCURATE DESIGN AND SIMULATION OF MULTIBODY SYSTEMS", Bhalerao K.D,
Rensselaer Polytechnic Institute, 2010.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

1.) Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.

2.) Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation and/or
other materials provided with the distribution.

3.) The name of the authors, their employers, or sponsoring organizations may
not be used to endorse or promote products derived from this software without
specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO
EVENT SHALL THE AUTHORS, THEIR EMPLOYERS, OR SPONSORING ORGANIZATIONS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
***************************************************************************/

#include "Assembly.h"
#include <fstream>

using namespace SimTK;
using namespace std;

// Motion constraints on the first residue
extern mCon PresMo1; // On Qindex 4 of residue 1
extern mCon PresMo2; // On Qindex 5 of residue 2
extern mCon PresMo3; // On Qindex 6 of residue 2


void Assembly::writeFreeJointList(std::ostream& OutputStream){
	for(int i=0;i<nBodies_-1;++i)
		OutputStream<<FreeJoints_[i]<<"\t";		
}

void Assembly::projectMomenta(){
	system_.realize(state_,Stage::Velocity);
	MatOp op;

	// This is a slightly tricky issue, The same variable is
	// used to store the values of constraint forces and impulses.
	// Must clear existing values before reusing the variable	
	
	CF_.clear();
	CF_.Init(nBodies_);
	
	// allocate space to store the values of deltav for each handle 	
	Dv_.clear();
 	Dv_.Init(nBodies_);

  	Vec6 *dvH1 = Dv_.getH1Ptr();
  	Vec6 *dvH2 = Dv_.getH2Ptr();
	
	const DcaBodyData* Bod = DcaObj_.getSystemPtr(); // returns body which contains all phi's and geometries
	
	//bool JointTransition=false;
	// Number of Composite bodies in the system
	int nCompositeBodies = nBodies_-2;
	CompositeBody *CompBody = new CompositeBody[nCompositeBodies];
	
	CompBody[0].formCompositeBody_projectMom(1,2,0,matter_,state_,DcaObj_,&FreeJoints_);
	
	for(int i=1;i<nCompositeBodies;++i)
		CompBody[i].formCompositeBody_projectMom(CompBody[i-1],i+2,i,matter_,state_,DcaObj_,&FreeJoints_);
	
		
	// Motion is specified on x and y coordinates of body 1
	// hence \Delta V is zero in that particular motion map 
	Mat63 Hcon; Hcon=0;
	Hcon(3,0)=Hcon(4,1)=Hcon(5,2)=1.0;	
	
	const Mat66& psi11 = CompBody[nCompositeBodies-1].getEomData().psi11;
	const Mat66& psi21 = CompBody[nCompositeBodies-1].getEomData().psi21;
	
	const Vec6& psi13 = CompBody[nCompositeBodies-1].getEomData().psi13;
	const Vec6& psi23 = CompBody[nCompositeBodies-1].getEomData().psi23;
	
	Vec3 RR1;
	RR1(0) = -psi13(3);
	RR1(1) = -psi13(4);
	RR1(2) = -psi13(5);
	
	Mat33 phi11,iphi11;

	for (int i = 0 ; i < 3 ; i++){
		for (int j = 0 ; j < 3 ; j++){
			phi11(i,j) = psi11(i+3,j+3);
		}
	}
	
	/*phi11(0,0) = psi11(3,3);
	phi11(0,1) = psi11(3,4);
	phi11(1,0) = psi11(4,3);
	phi11(1,1) = psi11(4,4);*/	
		
	Matrix phi11_r = op.toMatrix(phi11);
	SimTK::FactorLU lu(phi11_r);
	Matrix iphi11_r ;
	lu.inverse(iphi11_r);

	iphi11 = op.toMat33(iphi11_r);

	//// Manually calculate inverse of phi11_N_r
	//Real detVal = phi11(0,0)*phi11(1,1) - phi11(0,1)*phi11(1,0);
	//iphi11(0,0) = phi11(1,1)/detVal;
	//iphi11(0,1) = -phi11(0,1)/detVal;
	//iphi11(1,0) = -phi11(1,0)/detVal;
	//iphi11(1,1) = phi11(0,0)/detVal;	
		
	Vec6 *FcH1 = CF_.getFcH1Ptr();
	Vec6 *FcH2 = CF_.getFcH2Ptr();

	// Note, the values of contraint impules are both expressed in the mobilizer basis of the 
	// terminal body
	// Impulse on last body is zero
	FcH2[nBodies_-2]=0;
	
	// Impulse acting on first body
	FcH1[0]=Hcon*iphi11*RR1;
	
	Vec6 fch1_tmp;
	// Disassemble the system, Compute Constraint Impulses for all joints and the angular acceleration of all bodies
	for(int i=0;i<nBodies_-2;++i){
		
		// Constraint impulse on handle H1 of child body
		FcH1[nBodies_-2-i] = CompBody[nCompositeBodies-1-i].getA()*FcH1[0] + 
				    CompBody[nCompositeBodies-1-i].getB()*FcH2[nBodies_-i-2] + 
				    CompBody[nCompositeBodies-1-i].getC();
		
		dvH2[nBodies_-2-i] = Bod[nBodies_-2-i].psi21*FcH1[nBodies_-2-i]+Bod[nBodies_-2-i].psi22*FcH2[nBodies_-2-i]; 				      		 				
		dvH1[nBodies_-2-i] = Bod[nBodies_-2-i].psi11*FcH1[nBodies_-2-i]+Bod[nBodies_-2-i].psi12*FcH2[nBodies_-2-i];

 		const MobilizedBody& B1 = matter_.getMobilizedBody(MobilizedBodyIndex(nBodies_-2-i)); // Parent
 		const MobilizedBody& B2 = matter_.getMobilizedBody(MobilizedBodyIndex(nBodies_-1-i)); // Child
 		Mat33 Mp_C_M = (~B1.getOutboardFrame(state_).R())*B2.getInboardFrame(state_).R()*B2.getMobilizerTransform(state_).R();		
 
 		// Constraint impulse on handle H2 of parent body
 		op.expressVec6inBasis(FcH2[nBodies_-3-i],-FcH1[nBodies_-2-i],Mp_C_M);
 		
 		// basis of Fch1 has be shifted for each step
 		fch1_tmp = FcH1[0];
 		op.expressVec6inBasis(FcH1[0],fch1_tmp,Mp_C_M);
	}

	dvH1[0] = Bod[0].psi11*FcH1[0]+Bod[0].psi12*FcH2[0];
	dvH2[0] = Bod[0].psi21*FcH1[0]+Bod[0].psi22*FcH2[0];
		
	delete [] CompBody;
	CF_.clear();
	//Dv_.clear();	
	Assembly::updateState();
}

// Ideally, the code of this function should be written directly within the loop of 
// the function projectmomenta(), since the calculation of the matrix Mp_C_M is repeated
// here below. This is just done for the sake of clarity

void Assembly::updateState(){	
	MatOp op;

	Vec6 *dvH1 = Dv_.getH1Ptr();
  	Vec6 *dvH2 = Dv_.getH2Ptr();
	Vec6 tmpVec,tmp;
	
	// First body
	const MobilizedBody& body = matter_.getMobilizedBody(MobilizedBodyIndex(1));
	const Mat33& N_C_M = body.getMobilizerTransform(state_).R();
	
	op.expressVec6inBasis(tmpVec,dvH1[0],(N_C_M));	
	Real Utmp = body.getOneU(state_,2);
	state_.updU()[2]=tmpVec(2)+Utmp;
		
	for(int i=1;i<nBodies_-1;++i){

		if(FreeJoints_[i]==0){
			// if the joint is in transition. Lock it.
			FreeJoints_[i]=-1;
			state_.updU()[5+i]=0.0;
			continue;
		}
		else if(FreeJoints_[i]==-1){
			// Nothing to do, if the joint is already locked in
			// the previous state
			continue;
		}
		else if(FreeJoints_[i]==1){
			// This is the regular joint
			const MobilizedBody& B1 = matter_.getMobilizedBody(MobilizedBodyIndex(i)); // Parent
			const MobilizedBody& B2 = matter_.getMobilizedBody(MobilizedBodyIndex(i+1)); // Child
			Mat33 Mp_C_M = (~B1.getOutboardFrame(state_).R())*B2.getInboardFrame(state_).R()*B2.getMobilizerTransform(state_).R();
			
			op.expressVec6inBasis(tmpVec,dvH2[i-1],(~Mp_C_M));
			tmp = dvH1[i]-tmpVec;
			Utmp = B2.getOneU(state_,0);
			state_.updU()[5+i] = tmp(2)+Utmp;			
		}
	}	
}

void Assembly::AssembleSystem(void){
	
	Real time = state_.getTime();	
	
	
	// This is undesirable. Should be incorporated into an eventreporter()
	ofstream conFileH1, conFileH2 ;
	conFileH1.open("fh1.txt",ios::app);
	conFileH2.open("fh2.txt",ios::app);	
	
	MatOp op;		
	
	// Number of Composite bodies in the system
	int nCompositeBodies = nBodies_-2; // Excluding the ground too

	HandleEqs Eqns(system_,matter_,state_, Therm_); // To initialize phi13, phi23, and N_W_K

	
	Eqns.formHandleEquations(DcaObj_.getSystemPtr()/*to transfer body to the function*/); 
	         //formHandleEquations: Calculate psi13 and psi23 for each individual body at the Leaf level
	         // to fill Eqns members

	// The method is called to add the effect of the thermostat to psi13 and psi23
    Eqns.formThermostatHandleEqs(DcaObj_.getSystemPtr(),(const int**) &FreeJoints_ /* to be used in adding the torque from thermostat to psi13 and psi23*/ );

	TwoHandleEquations THE(DcaObj_,Eqns); // contains all the coefficients of all bodies in body's mobilizer frame
	
	CompositeBody *CompBody = new CompositeBody[nCompositeBodies];

	// Assemble the system
	CompBody[0].FormCompositeBody(1, 2,0,matter_,state_,THE,(const int**) &FreeJoints_);
	for(int i=1;i<nCompositeBodies;i++)
		CompBody[i].FormCompositeBody(CompBody[i-1],i+2,i,matter_,state_,THE,(const int**) &FreeJoints_);
	
	Mat63 H,D; H=D=0;
	H(0,0)=H(1,1)=H(2,2)=D(3,0)=D(4,1)=D(5,2)=1.0;
//	
//	// Motion is specified on x and y, z coordinates of residue 1
//	Mat63 Hcon; Hcon=0;
//	Hcon(3,0)=Hcon(4,1)=Hcon(5,2)=1.0;
//	
//	// Allocate space for n bodies
	Ang_.Init(nBodies_);
	CF_.clear();
	CF_.Init(nBodies_);

	Vec3 *AngAcc = Ang_.getPtr();
	Vec6 *FcH1 = CF_.getFcH1Ptr();
	Vec6 *FcH2 = CF_.getFcH2Ptr();
//	
//	// Recall that all Composite body data is written in the basis of the Mobilizer of the child 
//	// of the comprising two bodies
//	// Terminal body
	const MobilizedBody& B = matter_.getMobilizedBody(MobilizedBodyIndex(nBodies_-1));
	Mat33 N_C_M = B.getBodyRotation(state_)*B.getOutboardFrame(state_).R();
//	
//	// Calculate constraint force on the second handle of the last residue
//	FcH2[nBodies_-2]=0;
//	
//	
//	// Constraint force acting on first handle of the first residue
//	FcH1[0]=0; 		
//	
//	if (time>=1.5&&time<=2.0) // Within this time interval, we apply the enternal force on the 
//		                      // second handle of the terminal body.
//	{
//		Real Fmag=0;
//		Vec3 extforce(0);
//		
//		// This is the external force on terminal handle
//		// The force is applied in newtonian basis
////		Fmag = std::sin(20*(time-2))*50;
//		
//		FcH2[nBodies_-2](4)=0;		
//	}
//	else{
//		FcH2[nBodies_-2](4) = 0.0;
//	}
//
//	// Constraints on first body
//	const Real& currentTime = state_.getTime();
//	Real Ax = PresMo1.fdotdot(currentTime);
//	Real Ay = PresMo2.fdotdot(currentTime);
//	Real Az = PresMo3.fdotdot(currentTime);
//	
//	Mat66 psi11_N,psi12_N;Vec6 psi13_N,tmpvec6;
//	
//	// Convert psi11, psi12 and psi13 into newtonian basis since the available constraints on body 1
//	// are in newtonian basis. The weld constraint can be imposed on any basis since linear acceleration
//	// is zero in all the basis for that constraint.
//	op.expressMat66inBasis(psi11_N,CompBody[nCompositeBodies-1].getEomData().psi11,N_C_M);
// 	op.expressMat66inBasis(psi12_N,CompBody[nCompositeBodies-1].getEomData().psi12,N_C_M);	
//	op.expressVec6inBasis(psi13_N,CompBody[nCompositeBodies-1].getEomData().psi13,N_C_M);
//	
//	tmpvec6 = psi12_N*FcH2[nBodies_-2];
//	
//	// f1 represents the contraint force measures at handle 1 on resdiue 1
//	Vec3 RR1;
//	RR1(0) = Ax-psi13_N(3)-tmpvec6(3); 
//	RR1(1) = Ay-psi13_N(4)-tmpvec6(4); 
//	RR1(2) = Az-psi13_N(5)-tmpvec6(5); 
//	
//	Mat33 phi11_N_r;
//
//	for (int i = 0 ; i < 3 ; i++){
//		for (int j = 0 ; j < 3 ; j++){
//			phi11_N_r(i,j) = psi11_N(i+3,j+3);
//		}
//	}
//
//	/*std::cout << phi11_N_r << std::endl;*/
//	Matrix phi11_N_rr = op.toMatrix(phi11_N_r);
//	SimTK::FactorLU lu(phi11_N_rr);
//	Matrix iphi11_N_rr;
//	lu.inverse(iphi11_N_rr);
//
//	/*std::cout << iphi11_N_rr << std::endl;*/
//	
//	Mat33 iphi11_N_r = op.toMat33(iphi11_N_rr);
//
//	/*for (int i = 0 ; i < 3 ; i++){
//		for (int j = 0 ; j < 3 ; j++){
//			iphi11_N_r(i,j) = iphi11_N_rr(i,j);
//		}
//	}*/
//	
//	// fch1 is expressed in newtonian basis
	Vec6 fch1,fch2;
//	fch1 = Hcon*iphi11_N_r*RR1; // the required external force at H_1_1 to create the desired motion in
//	                            // x-direction and y-direction and z-direction
	
	fch1 = 0;
	FcH2[nBodies_-2] = 0; 
	fch2 = FcH2[nBodies_-2];

			
	// Note, the constraint force FcH1[0] is expressed in the basis of the 
	// mobilizer M of the terminal residue.	
	op.expressVec6inBasis(FcH1[0],fch1,(~N_C_M));
	op.expressVec6inBasis(FcH2[nBodies_-2],fch2,(~N_C_M));	
	//std::cout << nBodies_; 
	AngAcc[nBodies_-2] = (~H)*( CompBody[nCompositeBodies-1].getEomData().psi21*FcH1[0] + 
				    CompBody[nCompositeBodies-1].getEomData().psi22*FcH2[nBodies_-2] + 
				    CompBody[nCompositeBodies-1].getEomData().psi23 ); // Last Body
	
	Vec6 fch1_tmp;
	// Disassemble the system, Compute Constraint forces for all joints and the angular acceleration of all bodies
	for(int i=0;i<nBodies_-2;i++){
		
		// Constraint force on handle H1 of child body
		FcH1[nBodies_-2-i] = CompBody[nCompositeBodies-1-i].getA()*FcH1[0] + 
				    CompBody[nCompositeBodies-1-i].getB()*FcH2[nBodies_-i-2] + 
				    CompBody[nCompositeBodies-1-i].getC();

		const MobilizedBody& B1 = matter_.getMobilizedBody(MobilizedBodyIndex(nBodies_-2-i)); // Parent
		const MobilizedBody& B2 = matter_.getMobilizedBody(MobilizedBodyIndex(nBodies_-1-i)); // Child
		Mat33 Mp_C_M = (~B1.getOutboardFrame(state_).R())*B2.getInboardFrame(state_).R()*B2.getMobilizerTransform(state_).R();

		// Constraint force on handle H2 of parent body
		op.expressVec6inBasis(FcH2[nBodies_-3-i],-FcH1[nBodies_-2-i],Mp_C_M);
		
		// basis of Fch1 has be shifted for each step
		fch1_tmp = FcH1[0];
		op.expressVec6inBasis(FcH1[0],fch1_tmp,Mp_C_M);
		
		if(i==(nBodies_-3)){		
			const DcaBodyData *Mbs = THE.DcaObj.getSystemPtr();
			AngAcc[0] = (~H)*(Mbs[0].psi21*FcH1[0] + Mbs[0].psi22*FcH2[0] + THE.Eqns.getpsi23()[0]);
			LinAcc_ =   (~D)*(Mbs[0].psi11*FcH1[0] + Mbs[0].psi12*FcH2[0] + THE.Eqns.getpsi13()[0]);
			
			// The expression below was for the case with no constraints
			// LinAcc_ = (~D)*(Mbs[0].psi12*FcH2[0]+THE.Eqns.getpsi13()[0]);
			
		}
		else{
			AngAcc[nBodies_-i-3] = (~H)*( CompBody[nCompositeBodies-i-2].getEomData().psi21*FcH1[0] +
						     CompBody[nCompositeBodies-i-2].getEomData().psi22*FcH2[nBodies_-3-i] + 
						     CompBody[nCompositeBodies-i-2].getEomData().psi23 );
		}		
	}
	
	// Write contraint torques to the output files
	conFileH1<<state_.getTime()<<"\t";
	conFileH2<<state_.getTime()<<"\t";
	for(int i=0;i<nBodies_-1;++i){
		
		conFileH1<<FcH1[i](2)<<"\t";
		conFileH2<<FcH2[i](2)<<"\t";
// 		if (fabs(FcH1[i](2)>=ForceThreshold_)){
// 			printFreeJointList();
// 			cout<<endl<<i<<"\t"<<i+1<<endl;
// 			char ch;
// 			cin>>ch;
// 		}
			
		// Mine : We should be careful here, for the first body the FcH1 is not a constraint force,
		// it is an external force to create the prescribed motion, that value should not be compared 
		// with the ForceThreshold_
		if(this->FreeJoints_[i]==-1 && fabs(FcH1[i](2))>=ForceThreshold_){ // FcH1[i](2) = Torque along the joint axis
			// Joint released
			FreeJoints_[i]=1;
			releasedJoint_=true;
			cout<<"\nReleased joint between Mobilized body: "<< i <<" and " << i+1<<endl;
// 			char ch;
// 			cin>>ch;
		}

	}
	
	// Check for constraint torque magnitudes at locked joints
	
	conFileH1<<endl;
	conFileH2<<endl;

	conFileH1.close();
	conFileH2.close();
	
	// Not using Constraint forces anywhere. 
	// Comment this line out if the values of constraint forces are required		
	CF_.clear();
	delete [] CompBody;
}

void Assembly::calcUdot(Vector& Udot){
	Assembly::AssembleSystem();	
	/*ConstraintForce F ;
	Vec6 * F2  = CF_.getFcH2Ptr();
	cout << * F2 ;*/ // works if comment CF_.clear(); above
	Vec3 *Ang = Ang_.getPtr();
	for(int i = 0;i<nBodies_-1;++i){
		Vec3 RelAcc;	
		if(i==0){
			// First body
			const MobilizedBody& body = matter_.getMobilizedBody(MobilizedBodyIndex(1));
			const Mat33& N_C_M = body.getMobilizerTransform(state_).R();
			RelAcc = N_C_M*Ang[0];
			Udot(0)=RelAcc(0);
			Udot(1)=RelAcc(1);
			Udot(2)=RelAcc(2);
			Vec3 tmp = (N_C_M)*LinAcc_;
			//std::cout<<tmp;
			Udot(3)=tmp(0);
			Udot(4)=tmp(1);
			Udot(5)=tmp(2);
		}
		else{
			// Ang[i-1] written in basis of M of body (Referred to as Mp below)
			const MobilizedBody& body = matter_.getMobilizedBody(MobilizedBodyIndex(i));
			
			// Ang[i] written in basis of M of child
			const MobilizedBody& child = matter_.getMobilizedBody(MobilizedBodyIndex(i+1)); 
			Mat33 M_C_Mp =~((~body.getOutboardFrame(state_).R())*child.getInboardFrame(state_).R()*child.getMobilizerTransform(state_).R());

			Mat33 M_C_N =(~child.getOutboardFrame(state_).R())*(~child.getBodyRotation(state_));

			RelAcc = (Ang[i]-M_C_Mp*Ang[i-1]-M_C_N*(body.getBodyAngularVelocity(state_)%child.getBodyAngularVelocity(state_)));
			Udot(5+i) = RelAcc(2);		
		}
		
	}
}

void Assembly::calcQdotdot(Vector &Qdotdot){

	int nu = state_.getNU();
	Vector Udot(nu);
	Assembly::calcUdot(Udot);
	
	const MobilizedBody& B = matter_.getMobilizedBody(MobilizedBodyIndex(1));
	Vector Q1 = B.getQAsVector(state_);

	Vec4 BallQdotdot = calcQDotDotforBallJoint(B.getBodyAngularVelocity(state_), Vec3(Udot(0),Udot(1),Udot(2)),
												Vec4(Q1(0),Q1(1),Q1(2),Q1(3)));
	Qdotdot(0)=BallQdotdot(0);
	Qdotdot(1)=BallQdotdot(1);
	Qdotdot(2)=BallQdotdot(2);
	Qdotdot(3)=BallQdotdot(3);
	for(int i = 3;i<nu;i++)
		Qdotdot(4+i-3)=Udot(i);
}

void Assembly::calcQdotdotFromUdot(Vector& Qdotdot,const Vector& Udot){
	const MobilizedBody& B = matter_.getMobilizedBody(MobilizedBodyIndex(1));
	Vector Q1 = B.getQAsVector(state_);

	Vec4 BallQdotdot = calcQDotDotforBallJoint(B.getBodyAngularVelocity(state_), Vec3(Udot(0),Udot(1),Udot(2)),
												Vec4(Q1(0),Q1(1),Q1(2),Q1(3)));
	Qdotdot(0)=BallQdotdot(0);
	Qdotdot(1)=BallQdotdot(1);
	Qdotdot(2)=BallQdotdot(2);
	Qdotdot(3)=BallQdotdot(3);
	for(int i = 3;i<state_.getNU();i++)
		Qdotdot(4+i-3)=Udot(i);
}
